package brainbots.bot;

import brainbots.util.BBUtil;
import brainbots.util.Rotation;


public final class Motor{
    public double x,y,power;
    public double vx,vy;
    
    private double v_along, f_perp;
    
    public double u;
    public double force;
    public Motor(double x, double y, double vx, double vy){
        this.x = x;
        this.y = y;
        this.vx = vx;
        this.vy=vy;
        
        this.power = 1.0;
        
        calcProjections();
        
        u=0.0;
    }
    
    private void calcProjections()
    {/**Calculate projected value of force vector*/
        double mod_x = Math.sqrt(BBUtil.sqr(x) + BBUtil.sqr(y));
        if (mod_x >0){
            v_along = (vx*x + vy*y)/mod_x;
            f_perp = (x*vy - y*vx)/mod_x;
        }else{
            v_along = Math.sqrt(BBUtil.sqr(vx) + BBUtil.sqr(vy));
            f_perp = 0;
        }
        
        
    }
    
    public void set(double u){
        this.u = u;
    }
    public void simulate(double dt){
        //TODO
        force = u;
    }
    
    public void addForce(double[] f, Rotation phi){
    /**Add value of force, produced to the motor to the accumulator x*/
    
        //calcul;ate movement
        f[0] += phi.getRotatedX(x, y) * v_along * force;
        f[1] += phi.getRotatedY(x, y) * v_along * force;
    }
    
    public double getMoment()
    {
        return f_perp * force;
    }

	public double getEnergyConsumption() {
		return Math.abs(force);
	}
}